#include "GraphicsEngine.hpp"

int MPVStack::empty(){
    if( firstMPV == -1 ) return 1;
    return 0;
}

int MPVStack::full(){
    if( firstMPV == 99 ) return 1;
    return 0;
}

MPVStack::MPVStack(){
    firstMPV = -1;
}

mat4 *MPVStack::lookFirst(){
    return &MPV[firstMPV];
}

int MPVStack::add(mat4 _MPV){
    if(!full()){
        firstMPV++;
        MPV[firstMPV] = _MPV;

        return 0;
    }

    return 0;
}

int MPVStack::take(){
    if(!empty()){
        firstMPV--;
        return 0;
    }

    return -1;
}

int ChildStack::empty(){
    if( firstChild == -1 ) return 1;
    return 0;
}

int ChildStack::full(){
    if( firstChild == 99 ) return 1;
    return 0;
}

ChildStack::ChildStack(){
    firstChild = -1;
}

uint ChildStack::lookFirst(){
    if(empty()) return -1;
    return child[firstChild];
}

int ChildStack::addToFirst(){
    if(!full()){
        child[firstChild]++;

        return 0;
    }

    return 0;
}

int ChildStack::add(uint _child){
    if(!full()){
        firstChild++;
        child[firstChild] = _child;

        return 0;
    }

    return 0;
}

int ChildStack::take(){
    if(!empty()){
        firstChild--;

        return 0;
    }

    return -1;
}

int GraphicEngine::initWinodw(){
    // Initialise GLFW
	if( !glfwInit() )
	{
		cout << "Failed to initialize GLFW\n" << endl;
		return 0;
	}

	glfwOpenWindowHint(GLFW_FSAA_SAMPLES, 4);
	glfwOpenWindowHint(GLFW_OPENGL_VERSION_MAJOR, 3);
	glfwOpenWindowHint(GLFW_OPENGL_VERSION_MINOR, 3);
	glfwOpenWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);

	// Open a window and create its OpenGL context
	if( !glfwOpenWindow( 1024, 768, 0,0,0,0, 32,0, GLFW_WINDOW ) )
	{
		cout << "Failed to open GLFW window. If you have an Intel GPU, they are not 3.3 compatible. Try the 2.1 version of the tutorials.\n" << endl;
		glfwTerminate();
		return 0;
	}

	// Initialize GLEW
	glewExperimental = true; // Needed for core profile
	if (glewInit() != GLEW_OK) {
		cout << "Failed to initialize GLEW\n" << endl;
		return 0;
	}

	glfwSetWindowTitle( "Running free" );

	// Ensure we can capture the escape key being pressed below
	glfwEnable( GLFW_STICKY_KEYS );

    glEnable(GL_DEPTH_TEST);
    glDepthFunc(GL_LESS);

	// Dark blue background
	glClearColor(0.0f, 0.0f, 0.5f, 0.0f);

	glGenVertexArrays(1, &VertexArrayID);
	glBindVertexArray(VertexArrayID);

	return 1;
}

void GraphicEngine::compileShaders(){
    programID = LoadShaders( "SimpleVertexShader.vertexshader", "SimpleFragmentShader.fragmentshader" );
}

GraphicEngine::GraphicEngine() : bvh((char*)"models/Male1_C03_Run.bvh") {
    initWinodw();
    compileShaders();

    bvh.dumpTree();

    bmg = new BMG(&bvh);

    camera = 0;

    frameCount = 0;
    fps = 0;
    currentTime = 0;
    previousTime = 0;
}

void BMG::loadBoneToGPU(JOINT *joint){
    GLfloat bone[6];
    vec3 offset = joint->offset;
    int num = joint->num;

    bone[0] = 0.0;
    bone[1] = 0.0;
    bone[2] = 0.0;
    bone[3] = offset.x;
    bone[4] = offset.y;
    bone[5] = offset.z;

    glGenBuffers(1, &GPUVertexBuffer[num]);
    glBindBuffer(GL_ARRAY_BUFFER, GPUVertexBuffer[num]);
    glBufferData(GL_ARRAY_BUFFER,
                 sizeof(GLfloat) * 6,
                 bone,
                 GL_STATIC_DRAW);

    GLfloat boneColor[6];

    boneColor[0] = 0.0;
    boneColor[1] = 1.0;
    boneColor[2] = 0.0;
    boneColor[3] = 0.0;
    boneColor[4] = 1.0;
    boneColor[5] = 0.0;

    glGenBuffers(1, &GPUColorsBuffer[num]);
    glBindBuffer(GL_ARRAY_BUFFER, GPUColorsBuffer[num]);
    glBufferData(GL_ARRAY_BUFFER,
                 sizeof(GLfloat) * 6,
                 boneColor,
                 GL_STATIC_DRAW);
}

void BMG::loadBonesToGPU(){

    ChildStack childStack;
    JOINT *tj = bvh->root;

    childStack.add(0);
    childStack.add(0);
    tj = tj->childs[childStack.lookFirst()];

    while(!childStack.empty()){
        //cout << tj->name << ": " <<  tj->num << endl;
        if(tj->childsSize == childStack.lookFirst()){
            if(tj->childsSize == 0) loadBoneToGPU(tj);
            childStack.take();
            tj = tj->parent;
        } else {
            if(tj->num != -1)loadBoneToGPU(tj);
            tj = tj->childs[childStack.lookFirst()];
            childStack.addToFirst();
            childStack.add(0);
        }
    }
}

BMG::BMG(BVH *_bvh){
    bvh = _bvh;
    loadBonesToGPU();
}

void GraphicEngine::calculateFPS(){
    //  Increase frame count
    frameCount++;

    //  Get the number of milliseconds since glutInit called
    //  (or first call to glutGet(GLUT ELAPSED TIME)).
    currentTime = glutGet(GLUT_ELAPSED_TIME);

    //  Calculate time passed
    int timeInterval = currentTime - previousTime;

    if(timeInterval > 1000)
    {
        //  calculate the number of frames per second
        fps = frameCount / (timeInterval / 1000.0f);

        //  Set time
        previousTime = currentTime;

        //  Reset frame count
        frameCount = 0;
    }
}

void GraphicEngine::loadSurfaceToGPU(int plane){
    GLfloat surface[] = {
             0, 0,      0,
         plane, 0, -plane,
        -plane, 0, -plane,
        -plane, 0,  plane,
         plane, 0,  plane,
         plane, 0, -plane
    };

    glGenBuffers(1, &VBSurface);
    glBindBuffer(GL_ARRAY_BUFFER, VBSurface);
    glBufferData(GL_ARRAY_BUFFER,
                 sizeof(GLfloat) * 18,
                 surface,
                 GL_STATIC_DRAW);

    GLfloat surfaceColor[] = {
        0.0, 0.0, 0.2,
        0, 0, 0.5,
        0, 0, 0.5,
        0, 0, 0.5,
        0, 0, 0.5,
        0, 0, 0.5
    };

    glGenBuffers(1, &VCSurface);
    glBindBuffer(GL_ARRAY_BUFFER, VCSurface);
    glBufferData(GL_ARRAY_BUFFER,
                 sizeof(GLfloat) * 18,
                 surfaceColor,
                 GL_STATIC_DRAW);
}

void GraphicEngine::drawSurface(mat4 MPV, GLuint MatrixID){
    glUniformMatrix4fv(MatrixID, 1, GL_FALSE, &MPV[0][0]);

    // 1rst attribute buffer : vertices
    glEnableVertexAttribArray(0);
    glBindBuffer(GL_ARRAY_BUFFER, VBSurface);
    glVertexAttribPointer(
        0,                  // attribute. No particular reason for 0, but must match the layout in the shader.
        3,                  // size
        GL_FLOAT,           // type
        GL_FALSE,           // normalized?
        0,                  // stride
        (void*)0            // array buffer offset
    );

    // 2nd attribute buffer : colors
    glEnableVertexAttribArray(1);
    glBindBuffer(GL_ARRAY_BUFFER, VCSurface);
    glVertexAttribPointer(
        1,                                // attribute. No particular reason for 1, but must match the layout in the shader.
        3,                                // size
        GL_FLOAT,                         // type
        GL_FALSE,                         // normalized?
        0,                                // stride
        (void*)0                          // array buffer offset
    );

    glDrawArrays(GL_TRIANGLE_FAN, 0, 6);

    glDisableVertexAttribArray(0);
    glDisableVertexAttribArray(1);
}

void GraphicEngine::drawJOINT(JOINT *joint,
                              mat4 *lastMVP,
                              GLuint MatrixID,
                              uint frame,
                              GLuint GPUVertexBuffer,
                              GLuint GPUColorsBuffer){

    glUniformMatrix4fv(MatrixID, 1, GL_FALSE, &(*lastMVP)[0][0]);

    // 1rst attribute buffer : vertices
    glEnableVertexAttribArray(0);
    glBindBuffer(GL_ARRAY_BUFFER, GPUVertexBuffer);
    glVertexAttribPointer(
        0,                  // attribute. No particular reason for 0, but must match the layout in the shader.
        3,                  // size
        GL_FLOAT,           // type
        GL_FALSE,           // normalized?
        0,                  // stride
        (void*)0            // array buffer offset
    );

    // 2nd attribute buffer : colors
    glEnableVertexAttribArray(1);
    glBindBuffer(GL_ARRAY_BUFFER, GPUColorsBuffer);
    glVertexAttribPointer(
        1,                                // attribute. No particular reason for 1, but must match the layout in the shader.
        3,                                // size
        GL_FLOAT,                         // type
        GL_FALSE,                         // normalized?
        0,                                // stride
        (void*)0                          // array buffer offset
    );

    glDrawArrays(GL_LINE_STRIP, 0, 2);

    glDisableVertexAttribArray(0);
    glDisableVertexAttribArray(1);
}

void GraphicEngine::drawBVH(mat4 PV, GLuint MatrixID, int frame){
    JOINT *tj = bvh.root;

    MPVStack MPVstack;
    ChildStack childStack;

    vec3 rot;
    quat rotation;
    mat4 TranslationMatrix;
    mat4 MPV;

    childStack.add(0);

    ROOT *root = (ROOT*)tj;

    MPVstack.add(PV * translate(mat4(), root->position[frame]));

    rot = vec3(tj->channel[frame].x, tj->channel[frame].y, tj->channel[frame].z);
    rotation = normalize(quat(rot));
    TranslationMatrix = translate(mat4(), root->position[frame]);
    MPV = PV * TranslationMatrix * mat4_cast(rotation);

    MPVstack.add(MPV);

    tj = tj->childs[childStack.lookFirst()];

    childStack.addToFirst();
    childStack.add(0);

    while(!childStack.empty()){
        if(tj->childsSize == childStack.lookFirst()){
            if(tj->childsSize == 0) drawJOINT(tj, MPVstack.lookFirst(),
                                              MatrixID,
                                              frame,
                                              bmg->GPUVertexBuffer[tj->num],
                                              bmg->GPUColorsBuffer[tj->num]);
            childStack.take();
            MPVstack.take();

            tj = tj->parent;
        } else {
            if(tj->num != -1) drawJOINT(tj,
                                        MPVstack.lookFirst(),
                                        MatrixID,
                                        frame,
                                        bmg->GPUVertexBuffer[tj->num],
                                        bmg->GPUColorsBuffer[tj->num]);

            rot = vec3(tj->channel[frame].x, tj->channel[frame].y, tj->channel[frame].z);
            rotation = normalize(quat(rot));
            TranslationMatrix = translate(mat4(), tj->offset);
            MPV = (*MPVstack.lookFirst()) * TranslationMatrix * mat4_cast(rotation);

            tj = tj->childs[childStack.lookFirst()];

            MPVstack.add(MPV);
            childStack.addToFirst();
            childStack.add(0);
        }
    }
}

mat4 GraphicEngine::getHeadCamera(int frame){
    JOINT *tj = bvh.root;

    MPVStack MPVstack;
    ChildStack childStack;

    vec3 rot;
    quat rotation;
    mat4 TranslationMatrix;
    mat4 MPV;

    childStack.add(0);

    ROOT *root = (ROOT*)tj;

    MPVstack.add(translate(mat4(), root->position[frame]));

    rot = vec3(tj->channel[frame].x, tj->channel[frame].y, tj->channel[frame].z);
    rotation = normalize(quat(rot));
    TranslationMatrix = translate(mat4(), root->position[frame]);
    MPV = TranslationMatrix * mat4_cast(rotation);

    MPVstack.add(MPV);

    tj = tj->childs[childStack.lookFirst()];

    childStack.addToFirst();
    childStack.add(0);

    while(!childStack.empty()){
        if(tj->childsSize == childStack.lookFirst()){
            childStack.take();
            MPVstack.take();

            tj = tj->parent;
        } else {
            rot = vec3(tj->channel[frame].x, tj->channel[frame].y, tj->channel[frame].z);
            rotation = normalize(quat(rot));
            TranslationMatrix = translate(mat4(), tj->offset);
            MPV = (*MPVstack.lookFirst()) * TranslationMatrix * mat4_cast(rotation);

            tj = tj->childs[childStack.lookFirst()];
            if(!strcmp(tj->name, "Head")) {
                return MPV;
            }
            MPVstack.add(MPV);
            childStack.addToFirst();
            childStack.add(0);
        }
    }

    return MPV;
}

void GraphicEngine::graphicEngineLoop(){

    // Get a handle for our "MVP" uniform
	GLuint MatrixID = glGetUniformLocation(programID, "MVP");

	// Projection matrix : 45° Field of View, 4:3 ratio, display range : 0.1 unit <-> 10000 units
	Projection = perspective(45.0f, 4.0f / 3.0f, 0.1f, 10000.0f);
	// Camera matrix
    View       = lookAt(glm::vec3(0,1000,1), // Camera is at (4,3,-3), in World Space
						glm::vec3(0,0,0), // and looks at the origin
						glm::vec3(0,1,0)  // Head is up (set to 0,-1,0 to look upside-down)
						);

    vec3 position[1000];
    vec3 speed[1000];
    uint frame[1000];

    int max = 400;

    int planeSize = 5000;

    loadSurfaceToGPU(planeSize);

    for(int i = 0; i < max; i++){
        position[i].x = rand() % planeSize - planeSize * 1.5f;
        position[i].z = rand() % planeSize - planeSize * 1.5f;

        speed[i].x = rand() % 10 + 10;
        speed[i].z = speed[i].x;

        frame[i] = rand() % bvh.frames;
    }

    srand(time( NULL ));

    int st, te;

    int vkey = 1;

    int p = rand() % max;

    do{
        calculateFPS();

        st = glutGet(GLUT_ELAPSED_TIME);
        for(int i = 0; i < max; i++){
            if(frame[i] == bvh.frames - 1) frame[i] = 0;

            if(position[i].x > planeSize/2) position[i].x = rand() % planeSize - planeSize * 1.5f;
            if(position[i].z > planeSize/2) position[i].z = rand() % planeSize - planeSize * 1.5f;
            position[i].x += speed[i].x;
            position[i].z += speed[i].z;
        }

        switch(camera){
            case 0:
                View = lookAt(vec3(0,planeSize,1),
                              vec3(0,0,0),
                              vec3(0,1,0)
                              );
                break;
            case 1:
                View = lookAt(vec3(position[p].x - 200, position[p].y + 220, position[p].z - 200),
                              vec3(position[p].x, position[p].y + 150, position[p].z),
                              vec3(0,1,0)
                              );
                break;
            case 2:
                View = lookAt(vec3(position[p].x - 20, position[p].y + 200, position[p].z - 20),
                              vec3(position[p].x, position[p].y + 200, position[p].z),
                              vec3(0,1,0)
                              );
                View = getHeadCamera(frame[p]) * View;
               break;
        }

        //Clear the screen
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

		// Use our shader
		glUseProgram(programID);

        drawSurface(Projection * View, MatrixID);

        for(int i = 0; i < max; i++){
            drawBVH( Projection * View * translate(mat4(), position[i]), MatrixID, frame[i]);

            frame[i]++;
        }

		// Swap buffers
		glfwSwapBuffers();

		te = glutGet(GLUT_ELAPSED_TIME) - st;

		if(glfwGetKey( 86 ) == GLFW_PRESS && vkey){
		    cout << "Camera change" << endl;
		    p = rand() % max;
		    if(camera == 2) camera = 0;
		    else camera++;
		    vkey = 0;
		} else if(!glfwGetKey( 86 ) == GLFW_PRESS) vkey = 1;

        if((bvh.frameTime * 1000) - te> 0){
            usleep((bvh.frameTime * 1000 - te) *1000);
            //cout << te << endl;
        }
    }
	while( glfwGetWindowParam( GLFW_OPENED ) );
}

void GraphicEngine::destroyWindow(){
	// Cleanup VBO and shader

	glDeleteProgram(programID);

	glfwTerminate();
}
